#ifndef PREPROCESS_H
#define PREPROCESS_H

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PointStamped.h>
#include <std_msgs/Bool.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/Imu.h>
#include <image_transport/image_transport.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <yaml-cpp/yaml.h>

#include <nav_msgs/Odometry.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_datatypes.h> 

#include <Eigen/Eigen>
#include <Eigen/Dense>

#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
#include <cv_bridge/cv_bridge.h>

#include <multi_camera_cooperation/colors.h>
#include <multi_camera_cooperation/landmark.h>

#include <cstdlib>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <cstdio>
#include <fstream>
#include <cmath>
#include <ctime>
#include <queue>
#include <vector>
#include <chrono>
#include <sstream>


// #include "uav_config/read_config_drone.h"
#include "multi_camera_cooperation/math_tools.h"

class Preprocess
{
public:
    // camera parameters
    int camera_num;
    std::string camera_name = "camA";
    std::string servogroup_name = "servogroup12";
    std::string coopestimation_topic = " ";

    // flags
    bool OpticalFlowReadyFlag = false;
        
    // sub and pub
    ros::Subscriber sub_T_base_to_servogroup;
    ros::Subscriber sub_T_servogroup_to_cam;
    ros::Subscriber sub_T_base_to_coopestimation;
    ros::Subscriber sub_T_cam_to_estimation;
    ros::Subscriber sub_OpticalFlowReadyFlag;
    ros::Publisher pub_T_base_to_cam;
    ros::Publisher pub_T_base_to_cam_cal;
    ros::Publisher pub_T_base_to_servogroup;

    // transform
    Eigen::Matrix4d T_base_to_servogroup = Eigen::Matrix4d::Identity();
    Eigen::Matrix4d T_servogroup_to_cam = Eigen::Matrix4d::Identity();
    Eigen::Matrix4d T_base_to_coopestimation = Eigen::Matrix4d::Identity();
    Eigen::Matrix4d T_cam_to_estimation = Eigen::Matrix4d::Identity();
    Eigen::Matrix4d T_base_to_cam = Eigen::Matrix4d::Identity();
    Eigen::Matrix4d T_base_to_cam_pre = Eigen::Matrix4d::Identity();
    Eigen::Matrix4d T_base_to_cam_cal_raw = Eigen::Matrix4d::Identity();
    Eigen::Matrix4d T_base_to_cam_cal = Eigen::Matrix4d::Identity();
    tf::StampedTransform T_base_to_cam_tf;
    tf::StampedTransform T_base_to_servogroup_tf;

    // calibration
    tf::Vector3 mean_t;
    tf::Quaternion mean_q;
    int count = 0;

    // test
    ros::Publisher pub_T_base_to_cam_cal_raw;
    tf::StampedTransform T_base_to_cam_cal_raw_tf;
    ofstream outfile;

//================================== function declarations ==================================//
    Preprocess(ros::NodeHandle &nh);
    // ~Preprocess();

    void T_base_to_servogroup_callback(const geometry_msgs::TransformStamped::ConstPtr& msg);
    void T_servogroup_to_cam_callback(const geometry_msgs::TransformStamped::ConstPtr& msg);
    void T_base_to_coopestimation_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
    void T_cam_to_estimation_callback(const geometry_msgs::TransformStamped::ConstPtr& msg);
    void OpticalFlowReadyFlag_callback(const std_msgs::Bool::ConstPtr& msg);
    void publish_T_base_to_cam();
    void publish_T_base_to_cam_cal(bool pnpGoodFlag);
    void publish_T_base_to_servogroup();
};

#endif // PREPROCESS_H